﻿using System;
using System.Collections.Generic;
using System.Text;
using System.Diagnostics;
using System.Threading;
using System.Timers;
using WiimoteLib;

namespace SensorMash
{
    public class WiimoteData
    {
		private Wiimote mWiimote;
        private RawWiimoteVars rwmVars = new RawWiimoteVars();
        //private RawNunchukVars rncVars = new RawNunchukVars();

        /// <summary>
        /// Event raised when Sensor state is changed
        /// </summary>
        public event EventHandler<SensorMashChangedEventArgs> WiiSensorChanged;
        //public event EventHandler<SensorMashChangedEventArgs> NunchukSensorChanged;
        
        private Mutex m = new Mutex();

		public WiimoteData()
		{
#if DEBUG
            TextWriterTraceListener tr = new TextWriterTraceListener(System.IO.File.CreateText ("DebugLog\\" + System.DateTime.Now.ToString("yyMd_HHmm") + "_WiiMash.txt"));
            Debug.Listeners.Add(tr);
#endif

            Debug.WriteLine("-----WiimoteData-------");
            Debug.Flush();
	
            Debug.WriteLine("Wiimote initialisation");
            Debug.Flush();

            mWiimote = new Wiimote();
            mWiimote.WiimoteChanged += UpdateWiimoteChanged;
            mWiimote.WiimoteExtensionChanged += UpdateExtensionChanged;

            try
            {
                mWiimote.Connect();

                Debug.WriteLine("Wiimote is connected");
                Debug.Flush();
            }
            catch (Exception ex)
            {
                Debug.WriteLine(ex.Message);
                Debug.WriteLine(ex.StackTrace);
            }
            mWiimote.SetReportType(InputReport.IRAccel, true);
            mWiimote.SetLEDs(false, true, true, false);
            //wm.GetBatteryLevel();

            Debug.WriteLine("Wiimote is ready");
            Debug.Flush();
		}

        public void DisconnectWiimote()
        {
            mWiimote.Disconnect();
        }

		private void UpdateWiimoteChanged(object sender, WiimoteChangedEventArgs args)
		{
            m.WaitOne();

			WiimoteState ws = args.WiimoteState;

            rwmVars.varNameObjectMap["A"] = ws.ButtonState.A;
            rwmVars.varNameObjectMap["B"] = ws.ButtonState.B;
            rwmVars.varNameObjectMap["Minus"] = ws.ButtonState.Minus;
            rwmVars.varNameObjectMap["Home"] = ws.ButtonState.Home;
            rwmVars.varNameObjectMap["Plus"] = ws.ButtonState.Plus;
            rwmVars.varNameObjectMap["One"] = ws.ButtonState.One;
            rwmVars.varNameObjectMap["Two"] = ws.ButtonState.Two;
            rwmVars.varNameObjectMap["Up"] = ws.ButtonState.Up;
            rwmVars.varNameObjectMap["Down"] = ws.ButtonState.Down;
            rwmVars.varNameObjectMap["Left"] = ws.ButtonState.Left;
            rwmVars.varNameObjectMap["Right"] = ws.ButtonState.Right;
            rwmVars.varNameObjectMap["X"] = (double) ws.AccelState.Values.X;
            rwmVars.varNameObjectMap["Y"] = (double)ws.AccelState.Values.Y;
            rwmVars.varNameObjectMap["Z"] = (double) ws.AccelState.Values.Z;
            rwmVars.varNameObjectMap["LED1"] = ws.LEDState.LED1;
            rwmVars.varNameObjectMap["LED2"] = ws.LEDState.LED2;
            rwmVars.varNameObjectMap["LED3"] = ws.LEDState.LED3;
            rwmVars.varNameObjectMap["LED4"] = ws.LEDState.LED4;

            rwmVars.varNameObjectMap["ir0X"] = (double) ws.IRState.IRSensors[0].Position.X;
            rwmVars.varNameObjectMap["ir0Y"] = (double) ws.IRState.IRSensors[0].Position.Y;
            rwmVars.varNameObjectMap["ir1X"] = (double) ws.IRState.IRSensors[1].Position.X;
            rwmVars.varNameObjectMap["ir1Y"] = (double) ws.IRState.IRSensors[1].Position.Y;
            rwmVars.varNameObjectMap["ir2X"] = (double) ws.IRState.IRSensors[2].Position.X;
            rwmVars.varNameObjectMap["ir2Y"] = (double) ws.IRState.IRSensors[2].Position.Y;
            rwmVars.varNameObjectMap["ir3X"] = (double) ws.IRState.IRSensors[3].Position.X;
            rwmVars.varNameObjectMap["ir3Y"] = (double) ws.IRState.IRSensors[3].Position.Y;

            rwmVars.varNameObjectMap["irRaw0X"] = (double)ws.IRState.IRSensors[0].RawPosition.X;
            rwmVars.varNameObjectMap["irRaw0Y"] = (double)ws.IRState.IRSensors[0].RawPosition.Y;
            rwmVars.varNameObjectMap["irRaw1X"] = (double)ws.IRState.IRSensors[1].RawPosition.X;
            rwmVars.varNameObjectMap["irRaw1Y"] = (double)ws.IRState.IRSensors[1].RawPosition.Y;
            rwmVars.varNameObjectMap["irRaw2X"] = (double)ws.IRState.IRSensors[2].RawPosition.X;
            rwmVars.varNameObjectMap["irRaw2Y"] = (double)ws.IRState.IRSensors[2].RawPosition.Y;
            rwmVars.varNameObjectMap["irRaw3X"] = (double)ws.IRState.IRSensors[3].RawPosition.X;
            rwmVars.varNameObjectMap["irRaw3Y"] = (double)ws.IRState.IRSensors[3].RawPosition.Y;

            rwmVars.varNameObjectMap["ir0Found"] = ws.IRState.IRSensors[0].Found;
            rwmVars.varNameObjectMap["ir1Found"] = ws.IRState.IRSensors[1].Found;
            rwmVars.varNameObjectMap["ir2Found"] = ws.IRState.IRSensors[2].Found;
            rwmVars.varNameObjectMap["ir3Found"] = ws.IRState.IRSensors[3].Found;

            rwmVars.varNameObjectMap["ir0Size"] = (double)ws.IRState.IRSensors[0].Size;
            rwmVars.varNameObjectMap["ir1Size"] = (double)ws.IRState.IRSensors[1].Size;
            rwmVars.varNameObjectMap["ir2Size"] = (double)ws.IRState.IRSensors[2].Size;
            rwmVars.varNameObjectMap["ir3Size"] = (double)ws.IRState.IRSensors[3].Size;

            rwmVars.varNameObjectMap["Rumble"] = ws.Rumble;
            rwmVars.varNameObjectMap["Battery"] = (double) ws.Battery;

            calculateRollPitch((double)rwmVars.varNameObjectMap["X"], (double)rwmVars.varNameObjectMap["Y"], (double) rwmVars.varNameObjectMap["Z"], "WII");

            //rwmVars.A = ws.ButtonState.A;
            //rwmVars.B = ws.ButtonState.B;
            //rwmVars.Minus = ws.ButtonState.Minus;
            //rwmVars.Home = ws.ButtonState.Home;
            //rwmVars.Plus = ws.ButtonState.Plus;
            //rwmVars.One = ws.ButtonState.One;
            //rwmVars.Two = ws.ButtonState.Two;
            //rwmVars.Up = ws.ButtonState.Up;
            //rwmVars.Down = ws.ButtonState.Down;
            //rwmVars.Left = ws.ButtonState.Left;
            //rwmVars.Right = ws.ButtonState.Right;
            //rwmVars.X = ws.AccelState.Values.X;
            //rwmVars.Y = ws.AccelState.Values.Y;
            //rwmVars.Z = ws.AccelState.Values.Z;
            //rwmVars.LED1 = ws.LEDState.LED1;
            //rwmVars.LED2 = ws.LEDState.LED2;
            //rwmVars.LED3 = ws.LEDState.LED3;
            //rwmVars.LED4 = ws.LEDState.LED4;

            //rwmVars.ir0X = ws.IRState.IRSensors[0].Position.X;
            //rwmVars.ir0Y = ws.IRState.IRSensors[0].Position.Y;
            //rwmVars.ir1X = ws.IRState.IRSensors[1].Position.X;
            //rwmVars.ir1Y = ws.IRState.IRSensors[1].Position.Y;
            //rwmVars.ir2X = ws.IRState.IRSensors[2].Position.X;
            //rwmVars.ir2Y = ws.IRState.IRSensors[2].Position.Y;
            //rwmVars.ir3X = ws.IRState.IRSensors[3].Position.X;
            //rwmVars.ir3Y = ws.IRState.IRSensors[3].Position.Y;

            //rwmVars.Rumble = ws.Rumble;

			if (ws.ExtensionType == ExtensionType.Nunchuk)
			{
                    rwmVars.varNameObjectMap["NunC"] = ws.NunchukState.C;
                    rwmVars.varNameObjectMap["NunZ"] = ws.NunchukState.Z;
                    rwmVars.varNameObjectMap["NunXAcc"] = (double)ws.NunchukState.AccelState.Values.X;
                    rwmVars.varNameObjectMap["NunYAcc"] = (double)ws.NunchukState.AccelState.Values.Y;
                    rwmVars.varNameObjectMap["NunZAcc"] = (double)ws.NunchukState.AccelState.Values.Z;
                    rwmVars.varNameObjectMap["NunXJoystick"] = (double)ws.NunchukState.Joystick.X;
                    rwmVars.varNameObjectMap["NunYJoystick"] = (double)ws.NunchukState.Joystick.Y;
                    calculateRollPitch((double)rwmVars.varNameObjectMap["NunXAcc"], (double)rwmVars.varNameObjectMap["NunYAcc"], (double)rwmVars.varNameObjectMap["NunZAcc"], "NC");
                    //if (this.NunchukSensorChanged != null)
                    //{
                    //    this.NunchukSensorChanged(this, new SensorMashChangedEventArgs(rncVars));
                    //}                   
			}

            //foreach (KeyValuePair<string, object> kvp in rwmVars.varNameObjectMap)
            //{
            //    if (kvp.Value != null)
            //    {
            //        Debug.WriteLine(kvp.Key + "=" + kvp.Value.ToString());
            //        Debug.Flush();
            //    }
            //}

            if (this.WiiSensorChanged != null)
            {
                this.WiiSensorChanged(this, new SensorMashChangedEventArgs(rwmVars));
            }

            m.ReleaseMutex();
		}

        private void UpdateExtensionChanged(object sender, WiimoteExtensionChangedEventArgs args)
		{
            m.WaitOne();
			//rwmVars.ExtensionType = args.ExtensionType.ToString();
            rwmVars.varNameObjectMap["ExtensionType"] =  args.ExtensionType.ToString();
            rwmVars.varNameObjectMap["ExtensionInserted"] = args.Inserted;

            if (args.Inserted)
                mWiimote.SetReportType(InputReport.IRExtensionAccel, true);
            else
                mWiimote.SetReportType(InputReport.IRAccel, true);
            m.ReleaseMutex();
		}

        private void calculateRollPitch(double Xacc, double Yacc, double Zacc, string deviceType)
        {
            //    //  assume the controller is stationary if the acceleration vector is near
            //    //  1g for several updates (this may not always be correct)
            //    float length_sq = square(accel.X) + square(accel.Y) + square(accel.Z);

            double length_sq = Math.Pow(Xacc, 2) + Math.Pow(Yacc,2) + Math.Pow(Zacc,2);

//    // TODO: as I'm comparing _squared_ length, I really need different
//    //		  min/max epsilons...
//    #define DOT(x1,y1,z1, x2,y2,z2)	((x1*x2) + (y1*y2) + (z1*z2))

//    static const float epsilon = 0.2f;
//    if((length_sq >= (1.f-epsilon)) && (length_sq <= (1.f+epsilon)))
//        {
//        if(++WiimoteNearGUpdates < 2)
//            return false;
            		
//        // wiimote seems to be stationary:  normalize the current acceleration
//        //  (ie. the assumed gravity vector)
//        float inv_len = 1.f / sqrt(length_sq);
//        float x = accel.X * inv_len;
//        float y = accel.Y * inv_len;
//        float z = accel.Z * inv_len;

            double inv_len = 1 / Math.Sqrt(length_sq);
            double x = Xacc * inv_len;
            double y = Yacc * inv_len;
            double z = Zacc * inv_len;
                        
//        // copy the values
//        accel.Orientation.X = x;
//        accel.Orientation.Y = y;
//        accel.Orientation.Z = z;

//        // and extract pitch & roll from them:
//        // (may not be optimal)
//        float pitch = -asin(y) * 57.2957795f;
////		float roll  =  asin(x) * 57.2957795f;
//        float roll  =  atan2(x,z) * 57.2957795f;
//        if(z < 0) {
//            pitch = (y < 0)?  180 - pitch : -180 - pitch;
//            roll  = (x < 0)? -180 - roll  :  180 - roll;
//            }
            double pitch = Math.Asin(y) * 57.2957795 * -1;
            double roll = Math.Asin(x) * 57.2957795;
            if (z < 0)
            {
                if (y < 0)
                {
                    pitch = 180 - pitch;
                }
                else
                {
                    pitch = -180 - pitch;
                }

                if (x < 0)
                {
                    roll = -180 - roll;
                }
                else
                {
                    roll = 180 - roll;
                }
            }

            if (deviceType == "WII")
            {
                rwmVars.varNameObjectMap["Roll"] = roll;
                rwmVars.varNameObjectMap["Pitch"] = pitch;
            }
            else if (deviceType == "NC")
            {
                rwmVars.varNameObjectMap["NunRoll"] = roll;
                rwmVars.varNameObjectMap["NunPitch"] = pitch;
            }

//        accel.Orientation.Pitch = pitch;
//        accel.Orientation.Roll  = roll;

//        // show that we just updated orientation
//        accel.Orientation.UpdateAge = 0;
//#ifdef BEEP_ON_ORIENTATION_ESTIMATE
//        Beep(2000, 1);
//#endif
//        return true; // updated
//        }
        }

		public Wiimote Wiimote
		{
			set { mWiimote = value; }
		}

        public RawWiimoteVars RawWiimoteVars
        {
            get { return rwmVars; }
            set { rwmVars = value; }
        }

        //public RawNunchukVars RawNuncukVars
        //{
        //    get { return rncVars; }
        //    set { rncVars = value; }
        //}
    }
    public class RawWiimoteVars :SensorMashVars
    {
        //public bool A;
        //public bool B;
        //public bool Up;
        //public bool Down;
        //public bool Left;
        //public bool Right;
        //public bool Minus;
        //public bool Plus;
        //public bool Home;
        //public bool One;
        //public bool Two;
        //public bool LED1;
        //public bool LED2;
        //public bool LED3;
        //public bool LED4;
        //public bool Rumble;
        //public double X;
        //public double Y;
        //public double Z;
        //public double ir0X;
        //public double ir0Y;
        //public double ir1X;
        //public double ir1Y;
        //public double ir2X;
        //public double ir2Y;
        //public double ir3X;
        //public double ir3Y;
        //public double yaw;
        //public double pitch;
        //public double roll;
        //public string ExtensionType;
        //public bool inserted;

        public RawWiimoteVars()
        {
            //varNameObjectMap.Add("A", A);
            //varNameObjectMap.Add("B", B);
            //varNameObjectMap.Add("Up", Up);
            //varNameObjectMap.Add("Down", Down);
            //varNameObjectMap.Add("Left", Left);
            //varNameObjectMap.Add("Right", Right);
            //varNameObjectMap.Add("Minus", Minus);
            //varNameObjectMap.Add("Plus", Plus);
            //varNameObjectMap.Add("Home", Home);
            //varNameObjectMap.Add("One", One);
            //varNameObjectMap.Add("Two", Two);
            //varNameObjectMap.Add("X", X);
            //varNameObjectMap.Add("Y", Y);
            //varNameObjectMap.Add("Z", Z);
            //varNameObjectMap.Add("ir0X", ir0X);
            //varNameObjectMap.Add("ir0Y", ir0Y);
            //varNameObjectMap.Add("ir1X", ir1X);
            //varNameObjectMap.Add("ir1Y", ir1Y);
            //varNameObjectMap.Add("ir2X", ir2X);
            //varNameObjectMap.Add("ir2Y", ir2Y);
            //varNameObjectMap.Add("ir3X", ir3X);
            //varNameObjectMap.Add("ir3Y", ir3Y);
            //varNameObjectMap.Add("LED1", LED1);
            //varNameObjectMap.Add("LED2", LED2);
            //varNameObjectMap.Add("LED3", LED3);
            //varNameObjectMap.Add("LED4", LED4);
            //varNameObjectMap.Add("yaw", yaw);
            //varNameObjectMap.Add("pitch", pitch);
            //varNameObjectMap.Add("roll", roll);
            //varNameObjectMap.Add("ExtensionType", ExtensionType);
            //varNameObjectMap.Add("ExtensionInserted", inserted);
            //varNameObjectMap.Add("Rumble", Rumble);

            varNameObjectMap.Add("A", null);
            varNameObjectMap.Add("B", null);
            varNameObjectMap.Add("Up", null);
            varNameObjectMap.Add("Down", null);
            varNameObjectMap.Add("Left", null);
            varNameObjectMap.Add("Right", null);
            varNameObjectMap.Add("Minus", null);
            varNameObjectMap.Add("Plus", null);
            varNameObjectMap.Add("Home", null);
            varNameObjectMap.Add("One", null);
            varNameObjectMap.Add("Two", null);
            varNameObjectMap.Add("X", null);
            varNameObjectMap.Add("Y", null);
            varNameObjectMap.Add("Z", null);
            varNameObjectMap.Add("ir0X", null);
            varNameObjectMap.Add("ir0Y", null);
            varNameObjectMap.Add("ir1X", null);
            varNameObjectMap.Add("ir1Y", null);
            varNameObjectMap.Add("ir2X", null);
            varNameObjectMap.Add("ir2Y", null);
            varNameObjectMap.Add("ir3X", null);
            varNameObjectMap.Add("ir3Y", null);
            varNameObjectMap.Add("irRaw0X", null);
            varNameObjectMap.Add("irRaw0Y", null);
            varNameObjectMap.Add("irRaw1X", null);
            varNameObjectMap.Add("irRaw1Y", null);
            varNameObjectMap.Add("irRaw2X", null);
            varNameObjectMap.Add("irRaw2Y", null);
            varNameObjectMap.Add("irRaw3X", null);
            varNameObjectMap.Add("irRaw3Y", null);
            varNameObjectMap.Add("ir0Size", null);
            varNameObjectMap.Add("ir1Size", null);
            varNameObjectMap.Add("ir2Size", null);
            varNameObjectMap.Add("ir3Size", null);
            varNameObjectMap.Add("ir0Found", null);
            varNameObjectMap.Add("ir1Found", null);
            varNameObjectMap.Add("ir2Found", null);
            varNameObjectMap.Add("ir3Found", null);
            varNameObjectMap.Add("LED1", null);
            varNameObjectMap.Add("LED2", null);
            varNameObjectMap.Add("LED3", null);
            varNameObjectMap.Add("LED4", null);
            varNameObjectMap.Add("Yaw", null);
            varNameObjectMap.Add("Pitch", null);
            varNameObjectMap.Add("Roll", null);
            varNameObjectMap.Add("ExtensionType", null);
            varNameObjectMap.Add("ExtensionInserted", null);
            varNameObjectMap.Add("Rumble", null);
            varNameObjectMap.Add("Battery", null);
            
            //NUNCHUK VARS
            varNameObjectMap.Add("NunC", null);
            varNameObjectMap.Add("NunZ", null);
            varNameObjectMap.Add("NunXAcc", null);
            varNameObjectMap.Add("NunYAcc", null);
            varNameObjectMap.Add("NunZAcc", null);
            varNameObjectMap.Add("NunXJoystick", null);
            varNameObjectMap.Add("NunYJoystick", null);
            varNameObjectMap.Add("NunYaw", null);
            varNameObjectMap.Add("NunPitch", null);
            varNameObjectMap.Add("NunRoll", null);
        }

    }

    //public class RawNunchukVars: SensorMashVars
    //{
    //    public RawNunchukVars()
    //    {
    //        varNameObjectMap.Add("C", null);
    //        varNameObjectMap.Add("Z", null);
    //        varNameObjectMap.Add("XAcc", null);
    //        varNameObjectMap.Add("YAcc", null);
    //        varNameObjectMap.Add("ZAcc", null);
    //        varNameObjectMap.Add("XJoystick", null);
    //        varNameObjectMap.Add("YJoystick", null);
    //        varNameObjectMap.Add("Yaw", null);
    //        varNameObjectMap.Add("Pitch", null);
    //        varNameObjectMap.Add("Roll", null);
    //    }

    //    //public bool C;
    //    //public bool Z;
    //    //public double XAcc;
    //    //public double YAcc;
    //    //public double ZAcc;
    //    //public double XJoystick;
    //    //public double YJoystick;
    //    //public double yaw;
    //    //public double pitch;
    //    //public double roll;
    //}
}
